#include "mpu6050.h"

#include "stdio.h"

#define ROLL_PITCH_FIX_COUNT 25


mpu6050::mpu6050() {
	data_buf = 0;

	for (int i = 0; i < 12; i++) {
		mpuData[i] = 0;
	}

	//累加的数量
	acc_cnt = 0;
	gyr_cnt = 0;

	//加速度，三轴修正值
	acc_fixX = 0;
	acc_fixY = 0;
	acc_fixZ = 0;

	//角速度，三轴修正值
	gyr_fixX = 0;
	gyr_fixY = 0;
	gyr_fixZ = 0;


	//三轴加速度
	accx = 0;
	accy = 0;
	accz = 0;

	//三轴角速度
	gyrx = 0;
	gyry = 0;
	gyrz = 0;
	
	//roll pitch修正
	rollFix=0;
	pitchFix=0;
	lastUpdateT=0;

	//卡尔曼滤波
	m_fLastRoll = 0;
	m_fLastPitch = 0;
	
	//卡尔曼滤波参数设置
	m_kalmanRoll.setQangle(0.02);
	m_kalmanRoll.setQbias(0.02);
	m_kalmanRoll.setRmeasure(0.002);
	
	m_kalmanPitch.setQangle(0.02);
	m_kalmanPitch.setQbias(0.02);
	m_kalmanPitch.setRmeasure(0.002);
}


//mpu初始化
void mpu6050::init() {
	//初始化iic
	m_iic.Init(i2c1, 0xa1);		//这里的0xa1只是主机地址    	

	//mpu6050初始化
	write(PWR_MGMT_1, 0x0);      //正常启动
	write(SMPLRT_DIV, 0x7);      //陀螺仪采样率 125Hz
	write(MPU_CONFIG, 0x6);          //低通滤波频率 5Hz
	write(GYRO_CONFIG, 0x18);    //陀螺仪自检及测量范围，不自检，2000deg/s
	write(ACCEL_CONFIG, 0x1);    //加速计自检、测量范围及高通滤波频率 不自检，2G，5Hz
}

//更新传感器数据
void mpu6050::update() {
	//加速度
	mpuData[0] = read(ACCEL_XOUT_H);  //x轴
	mpuData[1] = read(ACCEL_XOUT_L);

	mpuData[2] = read(ACCEL_YOUT_H);  //y
	mpuData[3] = read(ACCEL_YOUT_L);

	mpuData[4] = read(ACCEL_ZOUT_H);  //z
	mpuData[5] = read(ACCEL_ZOUT_L);

	//陀螺仪(角速度)	
	mpuData[6] = read(GYRO_XOUT_H);   //x
	mpuData[7] = read(GYRO_XOUT_L);

	mpuData[8] = read(GYRO_YOUT_H);   //y
	mpuData[9] = read(GYRO_YOUT_L);

	mpuData[10] = read(GYRO_ZOUT_H);  //z
	mpuData[11] = read(GYRO_ZOUT_L);


	//数据转换	
	accx = acc_Cal(mpuData[0], mpuData[1]);
	accy = acc_Cal(mpuData[2], mpuData[3]);
	accz = acc_Cal(mpuData[4], mpuData[5]);

	gyrx = gyr_Cal(mpuData[6], mpuData[7]);
	gyry = gyr_Cal(mpuData[8], mpuData[9]);
	gyrz = gyr_Cal(mpuData[10], mpuData[11]);
}


//偏移修正部分
//添加累加值
void mpu6050::acc_add(float x, float y, float z) {
	acc_cnt++;
	acc_fixX += x;    //先用来累加
	acc_fixY += y;
	acc_fixZ += z;
}

//添加累加值
void mpu6050::gyr_add(float x, float y, float z) {
	gyr_cnt++;
	gyr_fixX += x;     //先用来累加
	gyr_fixY += y;
	gyr_fixZ += z;
}

//计算平均值
void mpu6050::acc_Average(void) {
	acc_fixX /= acc_cnt;
	acc_fixY /= acc_cnt;
	acc_fixZ /= acc_cnt;

	//z轴应为1倍重力加速度
	acc_fixZ -= 1;
}

//计算平均值
void mpu6050::gyr_Average(void) {
	gyr_fixX /= gyr_cnt;
	gyr_fixY /= gyr_cnt;
	gyr_fixZ /= gyr_cnt;
}


//获取修正值
float mpu6050::getAccX() {
	return accx - acc_fixX;
}

float mpu6050::getAccY() {
	return accy - acc_fixY;
}

float mpu6050::getAccZ() {
	return accz - acc_fixZ;
}


float mpu6050::getGyrX() {
	return gyrx - gyr_fixX;
}

float mpu6050::getGyrY() {
	return gyry - gyr_fixY;
}

float mpu6050::getGyrZ() {
	return gyrz - gyr_fixZ;
}




//iic操作
void mpu6050::write(u8 addr, u8 data) {
	m_iic.master_write_reg(MPU6050_ADDR, addr, data);
}

u8 mpu6050::read(u8 addr) {
	m_iic.master_read_reg(MPU6050_ADDR, addr, data_buf);
	return data_buf;
}






//内部运算操作

//转换16位
u16 mpu6050::calData(u8 dh, u8 dl) {
	u16 val = 0;
	val = (u16)dh;
	val = val << 8 | (u16)dl;
	return val;
}

//计算加速度
float mpu6050::acc_Cal(u8 h, u8 l) {
	s16 v = 0;
	v = (s16)calData(h, l);
	return (1.0f / 16384.0f)*(float)v;
}

//计算角速度
float mpu6050::gyr_Cal(u8 h, u8 l) {
	s16 v = 0;
	v = (s16)calData(h, l);
	return (1000.0f / 16384.0f)*(float)v;
}


//轴初始化(需要静止执行)
void mpu6050::axisInit(s16 cnt) {
	s16 i = 0;

	delay_ms(50);
	for (i = 0; i < cnt; i++) {
		update();		//12字节数据	

		//累加
		acc_add(accx, accy, accz);
		gyr_add(gyrx, gyry, gyrz);
	}

	//求平均值
	acc_Average();
	gyr_Average();
}



//弧度转角度
float mpu6050::rad2deg(float rad) {
	return 180.0f / 3.14159f*rad;
}



//计算加速度向量的模
float mpu6050::getAcceleration() {
	return sqrtf(accx*accx + accy*accy + accz*accz);
}




//计算roll角
float mpu6050::getRoll() {
	float v = sqrtf(accx*accx + accz*accz);
	v = acosf(v / getAcceleration());

	//符号修正
	//当y值为正时，Roll角要取负值
	if (accy > 0)v = -v;
	return rad2deg(v);		//转为角度显示
}


//计算pitch角
float mpu6050::getPitch() {
	float v = sqrtf(accy*accy + accz*accz);
	v = acosf(v / getAcceleration());

	//符号修正
	//当x轴为负时，Pitch角要取负值
	if (accx < 0)v = -v;
	return rad2deg(v);		//转为角度显示
}



//卡尔曼roll
void mpu6050::getKalmanRoll(float dt_second, float & out_roll, float & out_rollRate)
{
	//roll
	out_roll = m_kalmanRoll.getAngle(getRoll(), getGyrX(), dt_second);
	out_rollRate = (out_roll - m_fLastRoll) / dt_second;	//角速度
	m_fLastRoll = out_roll;
}

//卡尔曼pitch
void mpu6050::getKalmanPitch(float dt_second, float & out_pitch, float & out_pitchRate)
{
	//pitch
	out_pitch = m_kalmanPitch.getAngle(getPitch(), getGyrY(), dt_second);
	out_pitchRate = (out_pitch - m_fLastPitch) / dt_second;		//角速度
	m_fLastPitch = out_pitch;
}


//roll pitch修正
void mpu6050::roll_pitch_fix(){
	float roll=0,pitch=0;
	
	//跳过前x次
	for(int i=0;i<20;i++){
		upRollPitch(roll,pitch);						
		delay_ms(20);
	}
	
	//统计ROLL_PITCH_FIX_COUNT次作为修正值
	float rollSum=0,pitchSum=0;
	for(int i=0;i<ROLL_PITCH_FIX_COUNT;i++){
		upRollPitch(roll,pitch);
		
		rollSum+=roll;
		pitchSum+=pitch;
		
		delay_ms(20);
	}
	//均值
	rollFix=rollSum/ROLL_PITCH_FIX_COUNT;
	pitchFix=pitchSum/ROLL_PITCH_FIX_COUNT;
	
	printf("rollFix:%f  pitchFix:%f \r\n",rollFix,pitchFix);
}



void mpu6050::upRollPitch(float& roll,float& pitch){
	if(lastUpdateT==0)lastUpdateT=GetTickCount();
	
	this->update();
		
	vu64 t=GetTickCount();
	float dt=(float)(t-lastUpdateT);
	dt/=1000;
	lastUpdateT=t;

	float rollRate=0;
	getKalmanRoll(dt,roll,rollRate);
	
	float pitchRate=0;
	getKalmanPitch(dt,pitch,pitchRate);
}



//更新roll pitch
void mpu6050::updateRollPitch(float& roll,float& pitch){
	upRollPitch(roll,pitch);
	
	//修正
	roll-=rollFix;
	pitch-=pitchFix;
	

}






